//
// Created by jm on 2021/5/28.
//

#ifndef JPS_PLANNER_BASE_H
#define JPS_PLANNER_BASE_H

#include <util/data_type.h>
#include <planner/jps_planner/jps_graph_search.h>

#include <map/grid_map.h>


/**
 * @brief Abstract base for planning
 */

class JPS {
 public:
  /**
   * @brief Simple constructor
   * @param verbose enable debug mode
   */
  explicit JPS(bool verbose = false);

  ///Set zslam_map zslam_util for collistion checking
  void setMapUtil(const std::shared_ptr<sz::NavigationMap> &map_util);
  /**
   * @brief Status of the planner
   *
   * 0 --- exit normally;
   * -1 --- no path found;
   * 1, 2 --- start or goal is not free.
   */
  int status() const;
//    ///Get the modified path
//    VectorPoint2f GetPath();
  VectorPoint2i GetMapRawPath();
  ///Get the raw path
  __attribute__((unused)) VectorPoint2f GetRawPath();

  ///Planning function
  bool Plan(const Pointf<2> &start, const Pointf<2> &goal, float_decimal_t eps);
  ///Get the nodes in open set
  __attribute__((unused)) VectorPoint2f GetOpenSet() const;
  ///Get the nodes in close set
  __attribute__((unused)) VectorPoint2f GetCloseSet() const;
  ///Get all the nodes
  __attribute__((unused)) VectorPoint2f GetAllSet() const;
 protected:
  ///Enabled for printing Info
  bool planner_verbose_;
  ///Flag indicating the success of planning
  int status_ = 0;
  ///Assume using 3D voxel zslam_map for all 2d and 3d planning
  std::shared_ptr<sz::NavigationMap> map_util_;
  ///The planner
  std::shared_ptr<sz::JPSGraphSearch> graph_search_;
  ///Raw path from planner
  VectorPoint2i map_raw_path_;
  ///Raw path from planner
  VectorPoint2f raw_path_;
};

#endif
